2024.3.6 連続型Riccat代数方程式の解法（Riccati微分方程式をEuler法で解く）

scipy.linalg.solve_continus_areを用いて解いた解と比較を行う。

code:care1.py

import numpy as np

from numpy.linalg import eig, inv

import sys

import matplotlib.pyplot as plt

from scipy.linalg import solve_continuous_are as care

###

m, c, k = 1.0, 1.0, 1.0 # 質量、減衰係数、ばね定数

###

n, m = B.shape

X = np.identity(n)

dt = 0.01

X_hist = []

print('A =', A)

print('B =', B)

print('Q =', Q)

print('R =', R)

print('n =', n, 'm =', m)

print('dt =', dt)

print('X(0) =', X)

for i in range(1000):

Xflat = X.flatten()

print(Xflat)

X_hist.append(X.flat)

Xd = A.T@X + X@A - X@B@inv(R)@B.T@X + Q

X = X + Xd*dt

X_hist = np.array(X_hist)

P = care(A, B, Q, R)

print('# Solution of Control', P)

print('# Solution of Iteration', X)

print('# Norm of (X_control - X_iteration)', np.linalg.norm(X - P))

plt.grid()

plt.show()

関数化した。繰り返し計算の回数などは適当に設定する必要がある。

code:care2.py

import numpy as np

from numpy.linalg import eig, inv

import sys

import matplotlib.pyplot as plt

from scipy.linalg import solve_continuous_are as care

def solve_care_iteration(A, B, Q, R):

n, _ = B.shape

X = np.identity(n)

dt = 0.01

X_hist = []

max_iter = 1000

print('A =', A)

print('B =', B)

print('Q =', Q)

print('R =', R)

print('dt =', dt)

print('X(0) =', X)

for i in range(max_iter):

Xflat = X.flatten()

print(Xflat)

X_hist.append(X.flat)

Xd = A.T@X + X@A - X@B@inv(R)@B.T@X + Q

X = X + Xd*dt

return X, np.array(X_hist)

###

m, c, k = 1.0, 1.0, 1.0 # 質量、減衰係数、ばね定数

###

P_iter, P_hist = solve_care_iteration(A, B, Q, R)

P_scipy = care(A, B, Q, R)

print('# Solution of Scipy', P_scipy)

print('# Solution of Iteration', P_iter)

print('# Norm of (X_scipy - X_iter)', np.linalg.norm(P_scipy - P_iter))

plt.grid()

plt.show()